#include "stdafx.h"

#include "Matrix4.h"

Matrix4::Matrix4()
{
	setMatrix(0,0,0,0,
			  0,0,0,0,
			  0,0,0,0,
			  0,0,0,0);
}

Matrix4::Matrix4( float *f)
{

	setMatrix(f[0],f[1],f[2],f[3],
			  f[4],f[5],f[6],f[7],
			  f[9],f[9],f[10],f[11],
			  f[12],f[13],f[14],f[15]);
}

Matrix4::Matrix4( const Matrix4& mt)
{
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m_matrix[i][j]=mt.m_matrix[i][j];
		}
	}
}

Matrix4::Matrix4( float _11, float _12, float _13, float _14, float _21, float _22, float _23, float _24, float _31, float _32, float _33, float _34, float _41, float _42, float _43, float _44 )
{
	setMatrix(_11,_12,_13,_14,
			  _21,_22,_23,_24,
			  _31,_32,_33,_34,
			  _41,_42,_43,_44);
}



void Matrix4::setMatrix( float _11, float _12, float _13, float _14, float _21, float _22, float _23, float _24, float _31, float _32, float _33, float _34, float _41, float _42, float _43, float _44 )
{
	//row1
	m_matrix[0][0]=_11;
	m_matrix[0][1]=_12;
	m_matrix[0][2]=_13;
	m_matrix[0][3]=_14;
	//row2
	m_matrix[1][0]=_21;
	m_matrix[1][1]=_22;
	m_matrix[1][2]=_23;
	m_matrix[1][3]=_24;
	//row3
	m_matrix[2][0]=_31;
	m_matrix[2][1]=_32;
	m_matrix[2][2]=_33;
	m_matrix[2][3]=_34;
	//row4
	m_matrix[3][0]=_41;
	m_matrix[3][1]=_42;
	m_matrix[3][2]=_43;
	m_matrix[3][3]=_44;
}
float& Matrix4::operator()( unsigned int Row, unsigned int Col )
{
	Col=Col%4;
	Row=Row%4;
	return m_matrix(Row,Col);
}

float Matrix4::operator()( unsigned int Row, unsigned int Col ) const
{
	Col=Col%4;
	Row=Row%4;
	return m_matrix[Row][Col];
}

/*void Matrix4::operator=( const Matrix4 &mat )
{
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m_matrix[i][j]=mat.m_matrix[i][j];
		}
	}
}*/

Matrix4 &Matrix4::operator*=( const float f)
{
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m_matrix[i][j]*=f;
		}
	}
	return *this;
}
Matrix4 &Matrix4::operator/=( const float f)
{
	if(f!=0)
	{
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				m_matrix[i][j]/=f;
			}
		}
	}
	return *this;
}


Matrix4 &Matrix4::operator+=( const Matrix4& mt)
{
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m_matrix[i][j]+=mt.m_matrix[i][j];
		}
	}
	return *this;
}

Matrix4 &Matrix4::operator-=( const Matrix4& mt)
{
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m_matrix[i][j]-=mt.m_matrix[i][j];
		}
	}
	return *this;
}
Matrix4 &Matrix4::operator*=( const Matrix4& mt)
{
	
	Matrix4 tmp;
	
	tmp.m_matrix[0][0]= m_matrix[0][0]*mt.m_matrix[0][0]
					  + m_matrix[0][1]*mt.m_matrix[1][0]
					  + m_matrix[0][2]*mt.m_matrix[2][0]
					  + m_matrix[0][3]*mt.m_matrix[3][0];
	tmp.m_matrix[0][1]= m_matrix[0][0]*mt.m_matrix[0][1] 
					  + m_matrix[0][1]*mt.m_matrix[1][1]
					  + m_matrix[0][2]*mt.m_matrix[2][1]
					  + m_matrix[0][3]*mt.m_matrix[3][1];
	tmp.m_matrix[0][2]= m_matrix[0][0]*mt.m_matrix[0][2] 
					  + m_matrix[0][1]*mt.m_matrix[1][2]
					  + m_matrix[0][2]*mt.m_matrix[2][2]
					  + m_matrix[0][3]*mt.m_matrix[3][2];
	tmp.m_matrix[0][3]= m_matrix[0][0]*mt.m_matrix[0][3] 
					  + m_matrix[0][1]*mt.m_matrix[1][3]
					  + m_matrix[0][2]*mt.m_matrix[2][3]
					  + m_matrix[0][3]*mt.m_matrix[3][3];
	//row 2
	tmp.m_matrix[1][0]= m_matrix[1][0]*mt.m_matrix[0][0]
					  + m_matrix[1][1]*mt.m_matrix[1][0]
					  + m_matrix[1][2]*mt.m_matrix[2][0]
					  + m_matrix[1][3]*mt.m_matrix[3][0];
	tmp.m_matrix[1][1]= m_matrix[1][0]*mt.m_matrix[0][1] 
					  + m_matrix[1][1]*mt.m_matrix[1][1]
					  + m_matrix[1][2]*mt.m_matrix[2][1]
					  + m_matrix[1][3]*mt.m_matrix[3][1];
	tmp.m_matrix[1][2]= m_matrix[1][0]*mt.m_matrix[0][2] 
					  + m_matrix[1][1]*mt.m_matrix[1][2]
					  + m_matrix[1][2]*mt.m_matrix[2][2]
					  + m_matrix[1][3]*mt.m_matrix[3][2];
	tmp.m_matrix[1][3]= m_matrix[1][0]*mt.m_matrix[0][3] 
					  + m_matrix[1][1]*mt.m_matrix[1][3]
					  + m_matrix[1][2]*mt.m_matrix[2][3]
					  + m_matrix[1][3]*mt.m_matrix[3][3];
    //row 3
	tmp.m_matrix[2][0]= m_matrix[2][0]*mt.m_matrix[0][0]
					  + m_matrix[2][1]*mt.m_matrix[1][0]
					  + m_matrix[2][2]*mt.m_matrix[2][0]
					  + m_matrix[2][3]*mt.m_matrix[3][0];
	tmp.m_matrix[2][1]= m_matrix[2][0]*mt.m_matrix[0][1] 
					  + m_matrix[2][1]*mt.m_matrix[1][1]
					  + m_matrix[2][2]*mt.m_matrix[2][1]
					  + m_matrix[2][3]*mt.m_matrix[3][1];
	tmp.m_matrix[2][2]= m_matrix[2][0]*mt.m_matrix[0][2] 
					  + m_matrix[2][1]*mt.m_matrix[1][2]
					  + m_matrix[2][2]*mt.m_matrix[2][2]
					  + m_matrix[2][3]*mt.m_matrix[3][2];
	tmp.m_matrix[2][3]= m_matrix[2][0]*mt.m_matrix[0][3] 
					  + m_matrix[2][1]*mt.m_matrix[1][3]
					  + m_matrix[2][2]*mt.m_matrix[2][3]
					  + m_matrix[2][3]*mt.m_matrix[3][3];
   //row 4
    tmp.m_matrix[3][0]= m_matrix[3][0]*mt.m_matrix[0][0]
					  + m_matrix[3][1]*mt.m_matrix[1][0]
					  + m_matrix[3][2]*mt.m_matrix[2][0]
					  + m_matrix[3][3]*mt.m_matrix[3][0];
	tmp.m_matrix[3][1]= m_matrix[3][0]*mt.m_matrix[0][1] 
					  + m_matrix[3][1]*mt.m_matrix[1][1]
					  + m_matrix[3][2]*mt.m_matrix[2][1]
					  + m_matrix[3][3]*mt.m_matrix[3][1];
	tmp.m_matrix[3][2]= m_matrix[3][0]*mt.m_matrix[0][2] 
					  + m_matrix[3][1]*mt.m_matrix[1][2]
					  + m_matrix[3][2]*mt.m_matrix[2][2]
					  + m_matrix[3][3]*mt.m_matrix[3][2];
	tmp.m_matrix[3][3]= m_matrix[3][0]*mt.m_matrix[0][3] 
					  + m_matrix[3][1]*mt.m_matrix[1][3]
					  + m_matrix[3][2]*mt.m_matrix[2][3]
					  + m_matrix[3][3]*mt.m_matrix[3][3];
	 
	*this=tmp;
	return *this;
}

Matrix4 Matrix4::operator*( const Matrix4& mt) const
{
	Matrix4 tmp;
	
	tmp.m_matrix[0][0]= m_matrix[0][0]*mt.m_matrix[0][0]
					  + m_matrix[0][1]*mt.m_matrix[1][0]
					  + m_matrix[0][2]*mt.m_matrix[2][0]
					  + m_matrix[0][3]*mt.m_matrix[3][0];
	tmp.m_matrix[0][1]= m_matrix[0][0]*mt.m_matrix[0][1] 
					  + m_matrix[0][1]*mt.m_matrix[1][1]
					  + m_matrix[0][2]*mt.m_matrix[2][1]
					  + m_matrix[0][3]*mt.m_matrix[3][1];
	tmp.m_matrix[0][2]= m_matrix[0][0]*mt.m_matrix[0][2] 
					  + m_matrix[0][1]*mt.m_matrix[1][2]
					  + m_matrix[0][2]*mt.m_matrix[2][2]
					  + m_matrix[0][3]*mt.m_matrix[3][2];
	tmp.m_matrix[0][3]= m_matrix[0][0]*mt.m_matrix[0][3] 
					  + m_matrix[0][1]*mt.m_matrix[1][3]
					  + m_matrix[0][2]*mt.m_matrix[2][3]
					  + m_matrix[0][3]*mt.m_matrix[3][3];
	//row 2
	tmp.m_matrix[1][0]= m_matrix[1][0]*mt.m_matrix[0][0]
					  + m_matrix[1][1]*mt.m_matrix[1][0]
					  + m_matrix[1][2]*mt.m_matrix[2][0]
					  + m_matrix[1][3]*mt.m_matrix[3][0];
	tmp.m_matrix[1][1]= m_matrix[1][0]*mt.m_matrix[0][1] 
					  + m_matrix[1][1]*mt.m_matrix[1][1]
					  + m_matrix[1][2]*mt.m_matrix[2][1]
					  + m_matrix[1][3]*mt.m_matrix[3][1];
	tmp.m_matrix[1][2]= m_matrix[1][0]*mt.m_matrix[0][2] 
					  + m_matrix[1][1]*mt.m_matrix[1][2]
					  + m_matrix[1][2]*mt.m_matrix[2][2]
					  + m_matrix[1][3]*mt.m_matrix[3][2];
	tmp.m_matrix[1][3]= m_matrix[1][0]*mt.m_matrix[0][3] 
					  + m_matrix[1][1]*mt.m_matrix[1][3]
					  + m_matrix[1][2]*mt.m_matrix[2][3]
					  + m_matrix[1][3]*mt.m_matrix[3][3];
    //row 3
	tmp.m_matrix[2][0]= m_matrix[2][0]*mt.m_matrix[0][0]
					  + m_matrix[2][1]*mt.m_matrix[1][0]
					  + m_matrix[2][2]*mt.m_matrix[2][0]
					  + m_matrix[2][3]*mt.m_matrix[3][0];
	tmp.m_matrix[2][1]= m_matrix[2][0]*mt.m_matrix[0][1] 
					  + m_matrix[2][1]*mt.m_matrix[1][1]
					  + m_matrix[2][2]*mt.m_matrix[2][1]
					  + m_matrix[2][3]*mt.m_matrix[3][1];
	tmp.m_matrix[2][2]= m_matrix[2][0]*mt.m_matrix[0][2] 
					  + m_matrix[2][1]*mt.m_matrix[1][2]
					  + m_matrix[2][2]*mt.m_matrix[2][2]
					  + m_matrix[2][3]*mt.m_matrix[3][2];
	tmp.m_matrix[2][3]= m_matrix[2][0]*mt.m_matrix[0][3] 
					  + m_matrix[2][1]*mt.m_matrix[1][3]
					  + m_matrix[2][2]*mt.m_matrix[2][3]
					  + m_matrix[2][3]*mt.m_matrix[3][3];
   //row 4
    tmp.m_matrix[3][0]= m_matrix[3][0]*mt.m_matrix[0][0]
					  + m_matrix[3][1]*mt.m_matrix[1][0]
					  + m_matrix[3][2]*mt.m_matrix[2][0]
					  + m_matrix[3][3]*mt.m_matrix[3][0];
	tmp.m_matrix[3][1]= m_matrix[3][0]*mt.m_matrix[0][1] 
					  + m_matrix[3][1]*mt.m_matrix[1][1]
					  + m_matrix[3][2]*mt.m_matrix[2][1]
					  + m_matrix[3][3]*mt.m_matrix[3][1];
	tmp.m_matrix[3][2]= m_matrix[3][0]*mt.m_matrix[0][2] 
					  + m_matrix[3][1]*mt.m_matrix[1][2]
					  + m_matrix[3][2]*mt.m_matrix[2][2]
					  + m_matrix[3][3]*mt.m_matrix[3][2];
	tmp.m_matrix[3][3]= m_matrix[3][0]*mt.m_matrix[0][3] 
					  + m_matrix[3][1]*mt.m_matrix[1][3]
					  + m_matrix[3][2]*mt.m_matrix[2][3]
					  + m_matrix[3][3]*mt.m_matrix[3][3];
	 
	return tmp;
}

Matrix4 Matrix4::operator*(const float f) const
{
	Matrix4 tmp;
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			tmp.m_matrix[i][j]=m_matrix[i][j]*f;
		}
	}
	return tmp;	
}
Matrix4 Matrix4::operator/( const float f) const
{
	Matrix4 tmp;
	if(f!=0)
	{
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				tmp.m_matrix[i][j]=m_matrix[i][j]/f;
			}
		}
	}
	return tmp;
}

Matrix4 Matrix4::operator+( const Matrix4& a) const
{
	Matrix4 tmp;
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			tmp.m_matrix[i][j]=m_matrix[i][j]+a.m_matrix[i][j];
		}
	}
	return tmp;

}

Matrix4 Matrix4::operator-( const Matrix4& a ) const
{
	Matrix4 tmp;
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			tmp.m_matrix[i][j]=m_matrix[i][j]-a.m_matrix[i][j];
		}
	}
	return tmp;
}

//-------------------------------------------------------------------------------
// FUNCTION : determinant() 
// INPUT : none
// OUTPUT : float value
// DESC : function return determinant of matrix 4*4
//-------------------------------------------------------------------------------
float Matrix4::determinant() const {
	return m_matrix[0][0]*subMatrix4(0,0).determinant() - m_matrix[0][1]*subMatrix4(0,1).determinant() + 
		m_matrix[0][2]*subMatrix4(0,2).determinant() - m_matrix[0][3]*subMatrix4(0,3).determinant();	
}


//-------------------------------------------------------------------------------
// FUNCTION : setIdentity 
// INPUT : none
// OUTPUT : matrix 4*4
// DESC : set matrix 4*4 to Identity matrix
//-------------------------------------------------------------------------------
void Matrix4::setIdentity()
{
	setMatrix(
		1,0,0,0,
		0,1,0,0,
		0,0,1,0,
		0,0,0,1);
}

void Matrix4::setTranslation(const float sx, const float sy,const float sz)
{
	setMatrix(
		1.0f ,	0.0f,	0.0f,	0.0f,
		0.0f,	1.0f,	0.0f,	0.0f,
		0.0f,	0.0f,	1.0f,	0.0f,
		sx,		sy,		sz,		1.0f);
}
void Matrix4::setRotateXAxis(const float rxAngle)
{
	setMatrix(
				  1.0f ,	0.0f,			0.0f,		0.0f,
				  0.0f,	cos(rxAngle),	sin(rxAngle),	0.0f,
				  0.0f,	-sin(rxAngle),	cos(rxAngle),	0.0f,
				  0.0f,	0.0f,			0.0f,			1.0f
			);
}
void Matrix4::setRotateYAxis(const float yAngle)
{
	setMatrix(
		cos(yAngle),	0.0f,	-sin(yAngle),	0.0f,
		0.0f,			1.0f,	0.0f,			0.0f,
		sin(yAngle),	0.0f,	cos(yAngle),	0.0f,
		0.0f,			0.0f,	0.0f,			1.0f);
}
void Matrix4::setRotateZAxis(const float zAngle)
{
	setMatrix(
		cos(zAngle),	sin(zAngle),0.0f,	0.0f,
		-sin(zAngle),	cos(zAngle),0.0f,	0.0f,
		0.0f,			0.0f,		1.0f,	0.0f,
		0.0f,			0.0f,		0.0f,	1.0f);
}
//void Matrix4::setRotateAxis( const Vector3& vecAxis, float radians)
//{
//	Matrix4 TempMatrix;
//	float cosAngle, sinAngle, invCosAngle, x, y, z, mag;
//
//	x = vecAxis.x; y = vecAxis.y; z = vecAxis.z;
//
//	mag = x * x + y * y + z * z;
//
//	if (fabsf(1.0f - mag) > 1e-5f) 
//	{
//		mag = sqrtf(mag);
//
//		if (fabsf(mag) < 1e-5f){goto aaaa;}// break;
//			//return *this;
//		mag = 1.0f / mag;
//
//		x *= mag; y *= mag; z *= mag;
//	} // End If
//
//	cosAngle    = cosf(radians);
//	sinAngle    = sinf(radians);
//	invCosAngle = 1.0f - cosAngle;
//
//	// Build a new rotation matrix
//	setMatrix(
//	//TempMatrix[1][1] = 
//	invCosAngle * x * x + cosAngle,
//	//TempMatrix[1][2] = 
//	invCosAngle * x * y + z * sinAngle,
//	//TempMatrix[1][3] = 
//	invCosAngle * x * z - y * sinAngle,
//	//TempMatrix[1][4]= 
//	0.0f,
//
//	//TempMatrix[2][1] = 
//	invCosAngle * x * y - z * sinAngle,
//	//TempMatrix[2][2] = 
//	invCosAngle * y * y + cosAngle,
//	//TempMatrix[2][3] = 
//	invCosAngle * y * z + x * sinAngle,
//	//TempMatrix[2][4] = 
//	0.0f,
//
//	//TempMatrix[3][1] = 
//	invCosAngle * x * z + y * sinAngle,
//	//TempMatrix[3][2] = 
//	invCosAngle * y * z - x * sinAngle,
//	//TempMatrix[3][3] = 
//	invCosAngle * z * z + cosAngle,
//	//TempMatrix[3][4] = 
//	0.0f,
//
//	//TempMatrix[4][1] = 
//	0.0f, 
//	//TempMatrix[4][2] = 
//	0.0f, 
//	//TempMatrix[4][3] = 
//	0.0f,
//	//TempMatrix[4][4] = 
//	1.0f);
//
//	// If we are rotating current values, multiply it by the TempMatrix
//aaaa:;
//	
//}

void Matrix4::setRotateAxis(const float angle, const Vector3& axis){
	Vector3 V = axis;
	V.normalize();
	float x =V.x;
	float y = V.y;
	float z = V.z;


	float s= sin(angle);
	float c= cos(angle);
	float t= 1-c;
	setMatrix(
	t*(x*x)+c,	t*x*y+s*z,		t*x*z-s*y,	0,
	t*x*y-s*z,	t*y*y+c,		t*y*z+s*x,	0,
	t*x*z+s*y,	t*y*z-s*x,		t*z*z+c,	0,
	0,			0,				0,			1);


	//setMatrix(
	//t*(x*x)+c,	t*x*y-s*z,		t*x*z+s*y,	0,
	//t*x*y+s*z,	t*y*y+c,		t*y*z-s*x,	0,
	//t*x*z-s*y,	t*y*z+s*x,		t*z*z+c,	0,
	//0,			0,				0,			1);
}
//void Matrix4::setRotateAxis( const float Angle,const float xAxis,const float yAxis,const float zAxis)
//{
//	float cosAngle, sinAngle, invCosAngle, x, y, z, mag;
//
//	x =  xAxis; y = yAxis; z = zAxis;
//
//	mag = x * x + y * y + z * z;
//
//	if (fabsf(1.0f - mag) > 1e-5f) 
//	{
//		mag = sqrtf(mag);
//
//		if (fabsf(mag) < 1e-5f) 
//			return ;
//		mag = 1.0f / mag;
//
//		x *= mag; y *= mag; z *= mag;
//	} 
//
//	cosAngle    = cosf(Angle);
//	sinAngle    = sinf(Angle);
//	invCosAngle = 1.0f - cosAngle;
//
//	m_matrix[0][0] = invCosAngle * x * x + cosAngle;
//	m_matrix[0][1] = invCosAngle * x * y + z * sinAngle;
//	m_matrix[0][2] = invCosAngle * x * z - y * sinAngle;
//	m_matrix[0][3] = 0.0f;
//
//	m_matrix[1][0] = invCosAngle * x * y - z * sinAngle;
//	m_matrix[1][1] = invCosAngle * y * y + cosAngle;
//	m_matrix[1][2] = invCosAngle * y * z + x * sinAngle;
//	m_matrix[1][3] = 0.0f;
//
//	m_matrix[2][0] = invCosAngle * x * z + y * sinAngle;
//	m_matrix[2][1] = invCosAngle * y * z - x * sinAngle;
//	m_matrix[2][2] = invCosAngle * z * z + cosAngle;
//	m_matrix[2][3] = 0.0f;
//
//	m_matrix[3][0] = 0.0f; 
//	m_matrix[3][1] = 0.0f; 
//	m_matrix[3][2] = 0.0f; 
//	m_matrix[3][3] = 1.0f;
//}
//

void Matrix4::setMultiply( const Matrix4& a,const Matrix4 &b )
{
	//*this=a*b;
	Matrix4 c=a*b;
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m_matrix[i][j]=c.m_matrix[i][j];
		}
	}

}

void Matrix4::setScaling( const float sx, const float sy,const float sz )
{
	setMatrix(
		sx ,	0.0f,	0.0f,	0.0f,
		0.0f,	sy ,	0.0f,	0.0f,
		0.0f,	0.0f,	sz ,	0.0f,
		0.0f,	0.0f,	0.0f,	1.0f);
}


//-------------------------------------------------------------------------------
// FUNCTION : setInvert() 
// INPUT : none
// OUTPUT : matrix 4*4
// DESC : function will be invert matrix 4*4 and return matrix 4*4 after invert
//-------------------------------------------------------------------------------
void Matrix4::setInvert()
{
	unsigned char i, k;
	float	detA= determinant();
	Matrix4 invmat;
	bool    sign= false;


	if (!detA) return;						
	for (i=0; i < 4; ++i, sign= !sign)
	{
		for (k=0; k < 4; ++k, sign= !sign) 
		{	
			sign ? invmat.m_matrix[i][k]= -subMatrix4(i,k).determinant()/detA:invmat.m_matrix[i][k]= subMatrix4(i,k).determinant()/detA;			
		}
	}
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			m_matrix[i][j]=invmat.m_matrix[i][j];
		}
	}
}
//-------------------------------------------------------------------------------
// FUNCTION : getRow(unsigned int r)
// INPUT : int r
// OUTPUT : Vector4
// DESC : get Column "r" of Matrix 4*4 
//-------------------------------------------------------------------------------
Vector4 Matrix4::getRow(unsigned int r)const
{
	return Vector4(m_matrix[r][0],m_matrix[r][1],m_matrix[r][2],m_matrix[r][3]);
}

//-------------------------------------------------------------------------------
// FUNCTION : getColumn( unsigned int c )
// INPUT : int c
// OUTPUT : Vector4
// DESC : get Column "c" of Matrix 4*4 
//-------------------------------------------------------------------------------
Vector4 Matrix4::getColumn( unsigned int c ) const
{
	return Vector4(m_matrix[0][c],m_matrix[1][c],m_matrix[2][c],m_matrix[3][c]);
}


//--------------------------------------------------------------------------------
// FUNCTION : subMatrix4( unsigned char i, unsigned char j )
// INPUT : float i,j
// OUTPUT : Matrix3
// DESC : Translate a Matrix 4*4 become a Matrix 3*3 by remove row "i" and col "j"
//--------------------------------------------------------------------------------
Matrix3 Matrix4::subMatrix4( const unsigned int i, const unsigned int j ) const
{
	switch(i)
	{
	case 0: switch(j)
			{
	case 0: return Matrix3(
				m_matrix[1][1],m_matrix[1][2],m_matrix[1][3], 
				m_matrix[2][1],m_matrix[2][2],m_matrix[2][3],
				m_matrix[3][1],m_matrix[3][2],m_matrix[3][3]);									   
	case 1: return Matrix3(
				m_matrix[0][1],m_matrix[0][2],m_matrix[0][3], 
				m_matrix[2][1],m_matrix[2][3],m_matrix[2][3],
				m_matrix[3][1],m_matrix[3][2],m_matrix[3][3]);									   
	case 2: return Matrix3(
				m_matrix[0][1],m_matrix[0][2],m_matrix[0][3], 
				m_matrix[1][1],m_matrix[1][2],m_matrix[1][3],
				m_matrix[3][1],m_matrix[3][2],m_matrix[3][3]);
	default:return Matrix3(
				m_matrix[0][1],m_matrix[0][2],m_matrix[0][3], // case 3
				m_matrix[1][1],m_matrix[1][2],m_matrix[1][3], 
				m_matrix[2][1],m_matrix[2][2],m_matrix[2][3]);
			} 
	case 1: switch(j)
			{
	case 0: return Matrix3(
				m_matrix[1][0],m_matrix[1][2],m_matrix[1][3], 
				m_matrix[2][0],m_matrix[2][2],m_matrix[2][3],
				m_matrix[3][0],m_matrix[3][2],m_matrix[3][3]);									   
	case 1: return Matrix3(
				m_matrix[0][0],m_matrix[0][2],m_matrix[0][3], 
				m_matrix[2][0],m_matrix[2][2],m_matrix[2][3],
				m_matrix[3][0],m_matrix[3][2],m_matrix[3][3]);									   
	case 2: return Matrix3(
				m_matrix[0][0],m_matrix[0][2],m_matrix[0][3], 
				m_matrix[1][0],m_matrix[1][2],m_matrix[1][3],
				m_matrix[3][0],m_matrix[3][2],m_matrix[3][3]);
	default:return Matrix3(
				m_matrix[0][0],m_matrix[0][2],m_matrix[0][3], // case 3
				m_matrix[1][0],m_matrix[1][2],m_matrix[1][3], 
				m_matrix[2][0],m_matrix[2][2],m_matrix[2][3]);
			} 
	case 2: switch(j) 
			{
	case 0: return Matrix3(
				m_matrix[1][0],m_matrix[1][1],m_matrix[1][3], 
				m_matrix[2][0],m_matrix[2][1],m_matrix[2][3],
				m_matrix[3][0],m_matrix[3][1],m_matrix[3][3]);									   
	case 1: return Matrix3(
				m_matrix[0][0],m_matrix[0][1],m_matrix[0][3], 
				m_matrix[2][0],m_matrix[2][1],m_matrix[2][3],
				m_matrix[3][0],m_matrix[3][1],m_matrix[3][3]);									   
	case 2: return Matrix3(
				m_matrix[0][0],m_matrix[0][1],m_matrix[0][3], 
				m_matrix[1][0],m_matrix[1][1],m_matrix[1][3],
				m_matrix[3][0],m_matrix[3][1],m_matrix[3][3]);
	default:return Matrix3(
				m_matrix[0][0],m_matrix[0][1],m_matrix[0][3],// case 3 
				m_matrix[1][0],m_matrix[1][1],m_matrix[1][3],
				m_matrix[2][0],m_matrix[2][1],m_matrix[2][3]);
			} 
	default: switch(j) // case 3
			 {
	case 0: return Matrix3(
				m_matrix[1][0],m_matrix[1][1],m_matrix[1][2], 
				m_matrix[2][0],m_matrix[2][1],m_matrix[2][2],
				m_matrix[3][0],m_matrix[3][1],m_matrix[3][2]);									   
	case 1: return Matrix3(
				m_matrix[0][0],m_matrix[0][1],m_matrix[0][2], 
				m_matrix[2][0],m_matrix[2][1],m_matrix[2][2],
				m_matrix[3][0],m_matrix[3][1],m_matrix[3][2]);									   
	case 2: return Matrix3(
				m_matrix[0][0],m_matrix[0][1],m_matrix[0][2], 
				m_matrix[1][0],m_matrix[1][1],m_matrix[1][2],
				m_matrix[3][0],m_matrix[3][1],m_matrix[3][2]);
	default:return Matrix3(
				m_matrix[0][0],m_matrix[0][1],m_matrix[0][2], //case 3
				m_matrix[1][0],m_matrix[1][1],m_matrix[1][2],
				m_matrix[2][0],m_matrix[2][1],m_matrix[2][2]);
			 } 
	}
}




#pragma region OUTSIDE CLASS METHOD, UTILITY FUNCION FOR MATRIX
///-----------------------------------------------------------------------
/// out side class method, utility function for Matrix
///-----------------------------------------------------------------------


//-----------------------------------------------------------------------
// FUNCTION : operator* (float f,Matrix &outMatrix)
// INPUT : float f,Matrix4 -> out_mt,
// OUTPUT : Matrix4 -> out_mt
// DESC : scaling matrix with value f
//-----------------------------------------------------------------------
Matrix4 operator*( float f, const Matrix4& mt)
{
	Matrix4 tmp;
	for(int i=0;i<4;i++)
	{
		for(int j=0;j<4;j++)
		{
			tmp.m_matrix[i][j]=mt.m_matrix[i][j]*f;
		}
	}
	return tmp;
}
//-----------------------------------------------------------------------
// FUNCION : IVector3MatrixMultiply( IVector3 &vecDest,const IVector3 &vecSrc,const Matrix4 &mat )
// INPUT : IVector destination, IVector Source, Matrix4 mat
// OITPUT : Boolean, return new IVector after calculate for Vector Destination 
// DESC : Multiply Vector with Matrix and return new vector
//-----------------------------------------------------------------------
bool IVector3MatrixMultiply( Vector3 &vecDest,const Vector3 &vecSrc,const Matrix4 &mat )
{
		float x = vecSrc.x*mat(0,0) + vecSrc.y*mat(1,0) + vecSrc.z* mat(2,0) + mat(3,0);
		float y = vecSrc.x*mat(0,1) + vecSrc.y*mat(1,1) + vecSrc.z* mat(2,1) + mat(3,1);
		float z = vecSrc.x*mat(0,2) + vecSrc.y*mat(1,2) + vecSrc.z* mat(2,2) + mat(3,2);
		float w = vecSrc.x*mat(0,3) + vecSrc.y*mat(1,3) + vecSrc.z* mat(2,3) + mat(3,3);
		// Prevent Divide by 0 case.
		if( fabsf( w ) < EPSILON ) return false;
		// Homogenize the coordinate.
		vecDest.x = x / w;
		vecDest.y = y / w;
		vecDest.z = z / w;
		return true;
}


//-----------------------------------------------------------------------
// FUNCTION : Matrix4Identity (Matrix4 &outMatrix)
// INPUT : Matrix4 -> out_mt
// OUTPUT : Matrix4 -> out_mt
// DESC : Return Identity Matrix
//-----------------------------------------------------------------------
Matrix4 &Matrix4Identity( Matrix4 & out_mt )
{
	out_mt.setMatrix(
		1.0f ,	0.0f,	0.0f,	0.0f,
		0.0f,	1.0f ,	0.0f,	0.0f,
		0.0f,	0.0f,	1.0f ,	0.0f,
		0.0f,	0.0f,	0.0f,	1.0f);
	return out_mt;
}

//-----------------------------------------------------------------------
// FUNCTION : Matrix4Scaling (Matrix4 &outMatrix)
// INPUT : Matrix4 -> out_mt, scaling x axis,y x axis and z axis
// OUTPUT : Matrix4 -> out_mt
// DESC : Return Scaling Matrix
//-----------------------------------------------------------------------
Matrix4 Matrix4Scaling( Matrix4 & out_mt,const float sx,const float sy,const float sz )
{
	out_mt.setMatrix(
		sx ,	0.0f,	0.0f,	0.0f,
		0.0f,	sy ,	0.0f,	0.0f,
		0.0f,	0.0f,	sz ,	0.0f,
		0.0f,	0.0f,	0.0f,	1.0f);
	return out_mt;
}


//-----------------------------------------------------------------------
// FUNCTION : Matrix4Transpose (Matrix4 &outMatrix)
// INPUT : Matrix4 -> mt,
// OUTPUT : Matrix4 -> mt
// DESC : transpose input matrix
//-----------------------------------------------------------------------
Matrix4 Matrix4Transpose( Matrix4 & mt )
{
	Matrix4 b;
	b(0,0)=mt(0,0); b(0,1)=mt(1,0); b(0,2)=mt(2,0); b(0,3)=mt(3,0);
	b(1,0)=mt(0,1); b(1,1)=mt(1,1); b(1,2)=mt(2,1); b(1,3)=mt(3,1);
	b(2,0)=mt(0,2); b(2,1)=mt(1,2); b(2,2)=mt(2,2); b(2,3)=mt(3,2);
	b(3,0)=mt(0,3); b(3,1)=mt(1,3); b(3,2)=mt(2,3); b(3,3)=mt(3,3);	
	mt=b;
	return b;  
}


//-----------------------------------------------------------------------
// FUNCTION : Matrix4Translation( Matrix4 & out_mt,const float sx, const float sy,const float sz )
// INPUT : Matrix4 out_mt,float xAngle,
// OUTPUT : Matrix4 out_mt
// DESC : return Matrix after translate X,Y,Z position
//-----------------------------------------------------------------------
Matrix4 Matrix4::Matrix4Translation( Matrix4 & out_mt,const float sx, const float sy,const float sz )
{
	out_mt.setMatrix(
		1.0f ,	0.0f,	0.0f,	0.0f,
		0.0f,	1.0f,	0.0f,	0.0f,
		0.0f,	0.0f,	1.0f,	0.0f,
		sx,		sy,		sz,		1.0f);
	return out_mt;
}

bool Matrix4::setLookAtLH(const Vector3& vecPos, const Vector3& vecLookAt, const Vector3& vecUp )
{

	Vector3   uAxis, vAxis, nAxis;

	// Obtain look direction
	nAxis = (vecLookAt - vecPos);
	nAxis.normalize();

	// Obtain best up vector.
	vAxis = vecUp - (nAxis * vecUp.dot(nAxis));

	// Ensure the vAxis is valid.
	if (vAxis.len() < 1e-5f) {
		vAxis.x =      - nAxis.y * nAxis.x;
		vAxis.y = 1.0f - nAxis.y * nAxis.y;
		vAxis.z =      - nAxis.y * nAxis.z;
		if ( vAxis.len() < 1e-5f ) {
			vAxis.x =      - nAxis.z * nAxis.x;
			vAxis.y =      - nAxis.z * nAxis.y;
			vAxis.z = 1.0f - nAxis.z * nAxis.z;
			// No Chance, Bail.
			if (vAxis.len() < 1e-5f) return false;
		} // End If
	} // End If

	// Normalize and obtain v Axis
	vAxis.normalize();
	uAxis = vAxis.cross( nAxis );

	// Set Matrix Values
	setMatrix(
		uAxis.x,vAxis.x,nAxis.x,0.0f,
		uAxis.y,vAxis.y,nAxis.y,0.0f,
		uAxis.z,vAxis.z,nAxis.z,0.0f,
		-vecPos.dot(uAxis), -vecPos.dot(vAxis),-vecPos.dot(nAxis),1.0f
		);
	return true;
}

bool Matrix4::setPerspectiveFovLH( float fovY, const float aspect, float zn, float zf )
{
	float cotFovY = 1.0f/tan((fovY) * 0.5f);

	float scalex = cotFovY/ aspect;
	float scaley = cotFovY;

	setMatrix(
		scalex ,	0.0f,		0.0f,		0.0f,
		0.0f,		scaley,		0.0f,		0.0f,
		0.0f,		0.0f,		zf/(zf-zn),	 1.0f,
		0.0f,		0.0f,		-zn * zf/(zf-zn),	0.0f);
	return true;
}


//-----------------------------------------------------------------------
// FUNCTION : Matrix4RotateXAxis( Matrix4 &out_mt,float xAngle)
// INPUT : Matrix4 out_mt,float xAngle,
// OUTPUT : Matrix4 out_mt
// DESC : return Rotate Matrix around X-Axis with value xAngle
//-----------------------------------------------------------------------
Matrix4 Matrix4RotateXAxis( Matrix4 &out_mt,const float rxAngle)
{
	out_mt.setMatrix(
		1.0f ,	0.0f,			0.0f,		0.0f,
		0.0f,	cos(rxAngle),	sin(rxAngle),	0.0f,
		0.0f,	-sin(rxAngle),	cos(rxAngle),	0.0f,
		0.0f,	0.0f,			0.0f,			1.0f);
	return out_mt;
}


//-----------------------------------------------------------------------
// FUNCTION : Matrix4RotateYAxis( Matrix4 &out_mt,float yAngle)
// INPUT : Matrix4 out_mt,float yAngle,
// OUTPUT : Matrix4 out_mt
// DESC : return Rotate Matrix around Y-Axis with value yAngle
//-----------------------------------------------------------------------
Matrix4 Matrix4RotateYAxis( Matrix4 &out_mt,const float yAngle )
{
	out_mt.setMatrix(
		cos(yAngle),	0.0f,	-sin(yAngle),	0.0f,
		0.0f,			1.0f,	0.0f,			0.0f,
		sin(yAngle),	0.0f,	cos(yAngle),	0.0f,
		0.0f,			0.0f,	0.0f,			1.0f);
	return out_mt;
}


//-----------------------------------------------------------------------
// FUNCTION : Matrix4RotateYAxis( Matrix4 &out_mt,float zAngle)
// INPUT : Matrix4 out_mt,float zAngle,
// OUTPUT : Matrix4 out_mt
// DESC : return Rotate Matrix around Z-Axis with value zAngle
//-----------------------------------------------------------------------
Matrix4 Matrix4RotateZAxis( Matrix4 &out_mt,const float zAngle )
{
	out_mt.setMatrix(
		cos(zAngle),	sin(zAngle),0.0f,	0.0f,
		-sin(zAngle),	cos(zAngle),0.0f,	0.0f,
		0.0f,			0.0f,		1.0f,	0.0f,
		0.0f,			0.0f,		0.0f,	1.0f);
	return out_mt;
}
void Matrix4::VectorTransformCoord( Vector3& pOut, const Vector3& pV, const Matrix4& pM )
{
	Vector3 temp;
	//temp.x=(*pM)(0,1);
	temp.x = pV.x * pM(0,0) + pV.y * pM(1,0) + pV.z * pM(2,0) + pM(3,0);
	temp.y = pV.x * pM(0,1) + pV.y * pM(1,1) + pV.z * pM(2,1) + pM(3,1);
	temp.z = pV.x * pM(0,2) + pV.y * pM(1,2) + pV.z * pM(2,2) + pM(3,2);
	pOut=temp;
}

void Matrix4::VectorTransformNormal( Vector3& pOut, const Vector3& pV, const Matrix4& pM )
{
	Vector3 temp;
	temp.x = pV.x * pM(0,0) + pV.y * pM(1,0) + pV.z * pM(2,0);
	temp.y = pV.x * pM(0,1) + pV.y * pM(1,1) + pV.z * pM(2,1);
	temp.z = pV.x * pM(0,2) + pV.y * pM(1,2) + pV.z * pM(2,2);
	pOut = temp;
}


#pragma  endregion